home *** CD-ROM | disk | FTP | other *** search
/ Atari Forever 4 / Atari Forever 4.zip / Atari Forever 4.iso / SERIE_AI / AI_018 / POVRAY3.LZH / 3DSPOV18 / SOURCE / VECT.C < prev    next >
C/C++ Source or Header  |  1995-05-20  |  13KB  |  541 lines
  1. #define __GNUC__
  2.  
  3. #include "portab.h"
  4. #include <math.h>
  5. #include <string.h>
  6. #include "vect.h"
  7.  
  8. #define EPSILON 1e-6
  9.  
  10. void   adjoint (Matrix mat);
  11. double det4x4 (Matrix mat);
  12. double det3x3 (double a1, double a2, double a3, double b1, double b2,
  13.            double b3, double c1, double c2, double c3);
  14. double det2x2 (double a, double b, double c, double d);
  15.  
  16.  
  17. void vect_init (Vector v, float  x, float  y, float  z)
  18. {
  19.     v[X] = x;
  20.     v[Y] = y;
  21.     v[Z] = z;
  22. }
  23.  
  24.  
  25. void vect_copy (Vector v1, Vector v2)
  26. {
  27.     v1[X] = v2[X];
  28.     v1[Y] = v2[Y];
  29.     v1[Z] = v2[Z];
  30. }
  31.  
  32.  
  33. int vect_equal (Vector v1, Vector v2)
  34. {
  35.     if (v1[X] == v2[X] && v1[Y] == v2[Y] && v1[Z] == v2[Z])
  36.     return 1;
  37.     else
  38.     return 0;
  39. }
  40.  
  41.  
  42. void vect_add (Vector v1, Vector v2, Vector v3)
  43. {
  44.     v1[X] = v2[X] + v3[X];
  45.     v1[Y] = v2[Y] + v3[Y];
  46.     v1[Z] = v2[Z] + v3[Z];
  47. }
  48.  
  49.  
  50. void vect_sub (Vector v1, Vector v2, Vector v3)
  51. {
  52.     v1[X] = v2[X] - v3[X];
  53.     v1[Y] = v2[Y] - v3[Y];
  54.     v1[Z] = v2[Z] - v3[Z];
  55. }
  56.  
  57.  
  58. void vect_scale (Vector v1, Vector v2, float  k)
  59. {
  60.     v1[X] = k * v2[X];
  61.     v1[Y] = k * v2[Y];
  62.     v1[Z] = k * v2[Z];
  63. }
  64.  
  65.  
  66. float vect_mag (Vector v)
  67. {
  68.     float mag = sqrt(v[X]*v[X] + v[Y]*v[Y] + v[Z]*v[Z]);
  69.  
  70.     return mag;
  71. }
  72.  
  73.  
  74. void vect_normalize (Vector v)
  75. {
  76.     float mag = vect_mag (v);
  77.  
  78.     if (mag > 0.0)
  79.     vect_scale (v, v, 1.0/mag);
  80. }
  81.  
  82.  
  83. float vect_dot (Vector v1, Vector v2)
  84. {
  85.     return (v1[X]*v2[X] + v1[Y]*v2[Y] + v1[Z]*v2[Z]);
  86. }
  87.  
  88.  
  89. void vect_cross (Vector v1, Vector v2, Vector v3)
  90. {
  91.     v1[X] = (v2[Y] * v3[Z]) - (v2[Z] * v3[Y]);
  92.     v1[Y] = (v2[Z] * v3[X]) - (v2[X] * v3[Z]);
  93.     v1[Z] = (v2[X] * v3[Y]) - (v2[Y] * v3[X]);
  94. }
  95.  
  96. void vect_min (Vector v1, Vector v2, Vector v3)
  97. {
  98.     v1[X] = (v2[X] < v3[X]) ? v2[X] : v3[X];
  99.     v1[Y] = (v2[Y] < v3[Y]) ? v2[Y] : v3[Y];
  100.     v1[Z] = (v2[Z] < v3[Z]) ? v2[Z] : v3[Z];
  101. }
  102.  
  103.  
  104. void vect_max (Vector v1, Vector v2, Vector v3)
  105. {
  106.     v1[X] = (v2[X] > v3[X]) ? v2[X] : v3[X];
  107.     v1[Y] = (v2[Y] > v3[Y]) ? v2[Y] : v3[Y];
  108.     v1[Z] = (v2[Z] > v3[Z]) ? v2[Z] : v3[Z];
  109. }
  110.  
  111.  
  112. /* Return the angle between two vectors */
  113. float vect_angle (Vector v1, Vector v2)
  114. {
  115.     float  mag1, mag2, angle, cos_theta;
  116.  
  117.     mag1 = vect_mag(v1);
  118.     mag2 = vect_mag(v2);
  119.  
  120.     if (mag1 * mag2 == 0.0)
  121.     angle = 0.0;
  122.     else {
  123.     cos_theta = vect_dot(v1,v2) / (mag1 * mag2);
  124.  
  125.     if (cos_theta <= -1.0)
  126.         angle = 180.0;
  127.     else if (cos_theta >= +1.0)
  128.         angle = 0.0;
  129.     else
  130.         angle = (180.0/M_PI) * acos(cos_theta);
  131.     }
  132.  
  133.     return angle;
  134. }
  135.  
  136.  
  137. void vect_print (FILE *f, Vector v, int dec, char sep)
  138. {
  139.     char fstr[] = "%.4f, %.4f, %.4f";
  140.  
  141.     if (dec < 0) dec = 0;
  142.     if (dec > 9) dec = 9;
  143.  
  144.     fstr[2]  = '0' + dec;
  145.     fstr[8]  = '0' + dec;
  146.     fstr[14] = '0' + dec;
  147.  
  148.     fstr[4]  = sep;
  149.     fstr[10] = sep;
  150.  
  151.     fprintf (f, fstr, v[X], v[Y], v[Z]);
  152. }
  153.  
  154.  
  155. /* Rotate a vector about the X, Y or Z axis */
  156. void vect_rotate (Vector v1, Vector v2, int axis, float angle)
  157. {
  158.     float  cosa, sina;
  159.  
  160.     cosa = cos ((M_PI/180.0) * angle);
  161.     sina = sin ((M_PI/180.0) * angle);
  162.  
  163.     switch (axis) {
  164.     case X:
  165.         v1[X] =  v2[X];
  166.         v1[Y] =  v2[Y] * cosa + v2[Z] * sina;
  167.         v1[Z] =  v2[Z] * cosa - v2[Y] * sina;
  168.         break;
  169.  
  170.     case Y:
  171.         v1[X] = v2[X] * cosa - v2[Z] * sina;
  172.         v1[Y] = v2[Y];
  173.         v1[Z] = v2[Z] * cosa + v2[X] * sina;
  174.         break;
  175.  
  176.     case Z:
  177.         v1[X] = v2[X] * cosa + v2[Y] * sina;
  178.         v1[Y] = v2[Y] * cosa - v2[X] * sina;
  179.         v1[Z] = v2[Z];
  180.         break;
  181.     }
  182. }
  183.  
  184.  
  185. /* Rotate a vector about a specific axis */
  186. void vect_axis_rotate (Vector v1, Vector v2, Vector axis, float angle)
  187. {
  188.     float  cosa, sina;
  189.     Matrix mat;
  190.  
  191.     cosa = cos ((M_PI/180.0) * angle);
  192.     sina = sin ((M_PI/180.0) * angle);
  193.  
  194.     mat[0][0] = (axis[X] * axis[X]) + ((1.0 - (axis[X] * axis[X]))*cosa);
  195.     mat[0][1] = (axis[X] * axis[Y] * (1.0 - cosa)) - (axis[Z] * sina);
  196.     mat[0][2] = (axis[X] * axis[Z] * (1.0 - cosa)) + (axis[Y] * sina);
  197.     mat[0][3] = 0.0;
  198.  
  199.     mat[1][0] = (axis[X] * axis[Y] * (1.0 - cosa)) + (axis[Z] * sina);
  200.     mat[1][1] = (axis[Y] * axis[Y]) + ((1.0 - (axis[Y] * axis[Y])) * cosa);
  201.     mat[1][2] = (axis[Y] * axis[Z] * (1.0 - cosa)) - (axis[X] * sina);
  202.     mat[1][3] = 0.0;
  203.  
  204.     mat[2][0] = (axis[X] * axis[Z] * (1.0 - cosa)) - (axis[Y] * sina);
  205.     mat[2][1] = (axis[Y] * axis[Z] * (1.0 - cosa)) + (axis[X] * sina);
  206.     mat[2][2] = (axis[Z] * axis[Z]) + ((1.0 - (axis[Z] * axis[Z])) * cosa);
  207.     mat[2][3] = 0.0;
  208.  
  209.     mat[3][0] = mat[3][1] = mat[3][2] = mat[3][3] = 0.0;
  210.  
  211.     vect_transform (v1, v2, mat);
  212. }
  213.  
  214.  
  215. /* Transform the given vector */
  216. void vect_transform (Vector v1, Vector v2, Matrix mat)
  217. {
  218.     Vector tmp;
  219.  
  220.     tmp[X] = (v2[X] * mat[0][0]) + (v2[Y] * mat[1][0]) + (v2[Z] * mat[2][0]) + mat[3][0];
  221.     tmp[Y] = (v2[X] * mat[0][1]) + (v2[Y] * mat[1][1]) + (v2[Z] * mat[2][1]) + mat[3][1];
  222.     tmp[Z] = (v2[X] * mat[0][2]) + (v2[Y] * mat[1][2]) + (v2[Z] * mat[2][2]) + mat[3][2];
  223.  
  224.     vect_copy (v1, tmp);
  225. }
  226.  
  227.  
  228. /* Create an identity matrix */
  229. void mat_identity (Matrix mat)
  230. {
  231.     int i, j;
  232.  
  233.     for (i = 0; i < 4; i++)
  234.     for (j = 0; j < 4; j++)
  235.         mat[i][j] = 0.0;
  236.  
  237.     for (i = 0; i < 4; i++)
  238.     mat[i][i] = 1.0;
  239. }
  240.  
  241.  
  242. void mat_copy (Matrix mat1, Matrix mat2)
  243. {
  244.     int i, j;
  245.  
  246.     for (i = 0; i < 4; i++)
  247.     for (j = 0; j < 4; j++)
  248.         mat1[i][j] = mat2[i][j];
  249. }
  250.  
  251.  
  252. /* Rotate a matrix about the X, Y or Z axis */
  253. void mat_rotate (Matrix mat1, Matrix mat2, int axis, float angle)
  254. {
  255.     Matrix mat;
  256.     float  cosa, sina;
  257.  
  258.     cosa = cos ((M_PI/180.0) * angle);
  259.     sina = sin ((M_PI/180.0) * angle);
  260.  
  261.     mat_identity (mat);
  262.  
  263.     switch (axis) {
  264.     case X:
  265.         mat[1][1] = cosa;
  266.         mat[1][2] = sina;
  267.         mat[2][1] = -sina;
  268.         mat[2][2] = cosa;
  269.         break;
  270.  
  271.     case Y:
  272.         mat[0][0] = cosa;
  273.         mat[0][2] = -sina;
  274.         mat[2][0] = sina;
  275.         mat[2][2] = cosa;
  276.         break;
  277.  
  278.     case Z:
  279.         mat[0][0] = cosa;
  280.         mat[0][1] = sina;
  281.         mat[1][0] = -sina;
  282.         mat[1][1] = cosa;
  283.         break;
  284.     }
  285.  
  286.     mat_mult (mat1, mat2, mat);
  287. }
  288.  
  289.  
  290. void mat_axis_rotate (Matrix mat1, Matrix mat2, Vector axis, float angle)
  291. {
  292.     float  cosa, sina;
  293.     Matrix mat;
  294.  
  295.     cosa = cos ((M_PI/180.0) * angle);
  296.     sina = sin ((M_PI/180.0) * angle);
  297.  
  298.     mat[0][0] = (axis[X] * axis[X]) + ((1.0 - (axis[X] * axis[X]))*cosa);
  299.     mat[0][1] = (axis[X] * axis[Y] * (1.0 - cosa)) - (axis[Z] * sina);
  300.     mat[0][2] = (axis[X] * axis[Z] * (1.0 - cosa)) + (axis[Y] * sina);
  301.     mat[0][3] = 0.0;
  302.  
  303.     mat[1][0] = (axis[X] * axis[Y] * (1.0 - cosa)) + (axis[Z] * sina);
  304.     mat[1][1] = (axis[Y] * axis[Y]) + ((1.0 - (axis[Y] * axis[Y])) * cosa);
  305.     mat[1][2] = (axis[Y] * axis[Z] * (1.0 - cosa)) - (axis[X] * sina);
  306.     mat[1][3] = 0.0;
  307.  
  308.     mat[2][0] = (axis[X] * axis[Z] * (1.0 - cosa)) - (axis[Y] * sina);
  309.     mat[2][1] = (axis[Y] * axis[Z] * (1.0 - cosa)) + (axis[X] * sina);
  310.     mat[2][2] = (axis[Z] * axis[Z]) + ((1.0 - (axis[Z] * axis[Z])) * cosa);
  311.     mat[2][3] = 0.0;
  312.  
  313.     mat[3][0] = mat[3][1] = mat[3][2] = mat[3][3] = 0.0;
  314.  
  315.     mat_mult (mat1, mat2, mat);
  316. }
  317.  
  318.  
  319. /*  mat1 <-- mat2 * mat3 */
  320. void mat_mult (Matrix mat1, Matrix mat2, Matrix mat3)
  321. {
  322.     float sum;
  323.     int   i, j, k;
  324.     Matrix result;
  325.  
  326.     for (i = 0; i < 4; i++) {
  327.     for (j = 0; j < 4; j++) {
  328.         sum = 0.0;
  329.  
  330.         for (k = 0; k < 4; k++)
  331.         sum = sum + mat2[i][k] * mat3[k][j];
  332.  
  333.         result[i][j] = sum;
  334.     }
  335.     }
  336.  
  337.     for (i = 0; i < 4; i++)
  338.     for (j = 0; j < 4; j++)
  339.         mat1[i][j] = result[i][j];
  340. }
  341.  
  342.  
  343. /*
  344.    Decodes a 3x4 transformation matrix into separate scale, rotation,
  345.    translation, and shear vectors. Based on a program by Spencer W.
  346.    Thomas (Graphics Gems II)
  347. */
  348. void mat_decode (Matrix mat, Vector scale,  Vector shear, Vector rotate,
  349.         Vector transl)
  350. {
  351.     int i;
  352.     Vector row[3], temp;
  353.  
  354.     for (i = 0; i < 3; i++)
  355.     transl[i] = mat[3][i];
  356.  
  357.     for (i = 0; i < 3; i++) {
  358.     row[i][X] = mat[i][0];
  359.     row[i][Y] = mat[i][1];
  360.     row[i][Z] = mat[i][2];
  361.     }
  362.  
  363.     scale[X] = vect_mag (row[0]);
  364.     vect_normalize (row[0]);
  365.  
  366.     shear[X] = vect_dot (row[0], row[1]);
  367.     row[1][X] = row[1][X] - shear[X]*row[0][X];
  368.     row[1][Y] = row[1][Y] - shear[X]*row[0][Y];
  369.     row[1][Z] = row[1][Z] - shear[X]*row[0][Z];
  370.  
  371.     scale[Y] = vect_mag (row[1]);
  372.     vect_normalize (row[1]);
  373.  
  374.     if (scale[Y] != 0.0)
  375.     shear[X] /= scale[Y];
  376.  
  377.     shear[Y] = vect_dot (row[0], row[2]);
  378.     row[2][X] = row[2][X] - shear[Y]*row[0][X];
  379.     row[2][Y] = row[2][Y] - shear[Y]*row[0][Y];
  380.     row[2][Z] = row[2][Z] - shear[Y]*row[0][Z];
  381.  
  382.     shear[Z] = vect_dot (row[1], row[2]);
  383.     row[2][X] = row[2][X] - shear[Z]*row[1][X];
  384.     row[2][Y] = row[2][Y] - shear[Z]*row[1][Y];
  385.     row[2][Z] = row[2][Z] - shear[Z]*row[1][Z];
  386.  
  387.     scale[Z] = vect_mag (row[2]);
  388.     vect_normalize (row[2]);
  389.  
  390.     if (scale[Z] != 0.0) {
  391.     shear[Y] /= scale[Z];
  392.     shear[Z] /= scale[Z];
  393.     }
  394.  
  395.     vect_cross (temp, row[1], row[2]);
  396.     if (vect_dot (row[0], temp) < 0.0) {
  397.     for (i = 0; i < 3; i++) {
  398.         scale[i]  *= -1.0;
  399.         row[i][X] *= -1.0;
  400.         row[i][Y] *= -1.0;
  401.         row[i][Z] *= -1.0;
  402.     }
  403.     }
  404.  
  405.     if (row[0][Z] < -1.0) row[0][Z] = -1.0;
  406.     if (row[0][Z] > +1.0) row[0][Z] = +1.0;
  407.  
  408.     rotate[Y] = asin(-row[0][Z]);
  409.  
  410.     if (fabs(cos(rotate[Y])) > EPSILON) {
  411.     rotate[X] = atan2 (row[1][Z], row[2][Z]);
  412.     rotate[Z] = atan2 (row[0][Y], row[0][X]);
  413.     }
  414.     else {
  415.     rotate[X] = atan2 (row[1][X], row[1][Y]);
  416.     rotate[Z] = 0.0;
  417.     }
  418.  
  419.     /* Convert the rotations to degrees */
  420.     rotate[X] = (180.0/M_PI)*rotate[X];
  421.     rotate[Y] = (180.0/M_PI)*rotate[Y];
  422.     rotate[Z] = (180.0/M_PI)*rotate[Z];
  423. }
  424.  
  425.  
  426. /* Matrix inversion code from Graphics Gems */
  427.  
  428. /* mat1 <-- mat2^-1 */
  429. float mat_inv (Matrix mat1, Matrix mat2)
  430. {
  431.     int i, j;
  432.     float det;
  433.  
  434.     if (mat1 != mat2) {
  435.     for (i = 0; i < 4; i++)
  436.         for (j = 0; j < 4; j++)
  437.         mat1[i][j] = mat2[i][j];
  438.     }
  439.  
  440.     det = det4x4 (mat1);
  441.  
  442.     if (fabs (det) < EPSILON)
  443.     return 0.0;
  444.  
  445.     adjoint (mat1);
  446.  
  447.     for (i = 0; i < 4; i++)
  448.     for(j = 0; j < 4; j++)
  449.         mat1[i][j] = mat1[i][j] / det;
  450.  
  451.     return det;
  452. }
  453.  
  454.  
  455. void adjoint (Matrix mat)
  456. {
  457.     double a1, a2, a3, a4, b1, b2, b3, b4;
  458.     double c1, c2, c3, c4, d1, d2, d3, d4;
  459.  
  460.     a1 = mat[0][0]; b1 = mat[0][1];
  461.     c1 = mat[0][2]; d1 = mat[0][3];
  462.  
  463.     a2 = mat[1][0]; b2 = mat[1][1];
  464.     c2 = mat[1][2]; d2 = mat[1][3];
  465.  
  466.     a3 = mat[2][0]; b3 = mat[2][1];
  467.     c3 = mat[2][2]; d3 = mat[2][3];
  468.  
  469.     a4 = mat[3][0]; b4 = mat[3][1];
  470.     c4 = mat[3][2]; d4 = mat[3][3];
  471.  
  472.     /* row column labeling reversed since we transpose rows & columns */
  473.     mat[0][0]  =  det3x3 (b2, b3, b4, c2, c3, c4, d2, d3, d4);
  474.     mat[1][0]  = -det3x3 (a2, a3, a4, c2, c3, c4, d2, d3, d4);
  475.     mat[2][0]  =  det3x3 (a2, a3, a4, b2, b3, b4, d2, d3, d4);
  476.     mat[3][0]  = -det3x3 (a2, a3, a4, b2, b3, b4, c2, c3, c4);
  477.  
  478.     mat[0][1]  = -det3x3 (b1, b3, b4, c1, c3, c4, d1, d3, d4);
  479.     mat[1][1]  =  det3x3 (a1, a3, a4, c1, c3, c4, d1, d3, d4);
  480.     mat[2][1]  = -det3x3 (a1, a3, a4, b1, b3, b4, d1, d3, d4);
  481.     mat[3][1]  =  det3x3 (a1, a3, a4, b1, b3, b4, c1, c3, c4);
  482.  
  483.     mat[0][2]  =  det3x3 (b1, b2, b4, c1, c2, c4, d1, d2, d4);
  484.     mat[1][2]  = -det3x3 (a1, a2, a4, c1, c2, c4, d1, d2, d4);
  485.     mat[2][2]  =  det3x3 (a1, a2, a4, b1, b2, b4, d1, d2, d4);
  486.     mat[3][2]  = -det3x3 (a1, a2, a4, b1, b2, b4, c1, c2, c4);
  487.  
  488.     mat[0][3]  = -det3x3 (b1, b2, b3, c1, c2, c3, d1, d2, d3);
  489.     mat[1][3]  =  det3x3 (a1, a2, a3, c1, c2, c3, d1, d2, d3);
  490.     mat[2][3]  = -det3x3 (a1, a2, a3, b1, b2, b3, d1, d2, d3);
  491.     mat[3][3]  =  det3x3 (a1, a2, a3, b1, b2, b3, c1, c2, c3);
  492. }
  493.  
  494.  
  495. double det4x4 (Matrix mat)
  496. {
  497.     double ans;
  498.     double a1, a2, a3, a4, b1, b2, b3, b4, c1, c2, c3, c4, d1, d2, d3,             d4;
  499.  
  500.     a1 = mat[0][0]; b1 = mat[0][1];
  501.     c1 = mat[0][2]; d1 = mat[0][3];
  502.  
  503.     a2 = mat[1][0]; b2 = mat[1][1];
  504.     c2 = mat[1][2]; d2 = mat[1][3];
  505.  
  506.     a3 = mat[2][0]; b3 = mat[2][1];
  507.     c3 = mat[2][2]; d3 = mat[2][3];
  508.  
  509.     a4 = mat[3][0]; b4 = mat[3][1];
  510.     c4 = mat[3][2]; d4 = mat[3][3];
  511.  
  512.     ans = a1 * det3x3 (b2, b3, b4, c2, c3, c4, d2, d3, d4) -
  513.       b1 * det3x3 (a2, a3, a4, c2, c3, c4, d2, d3, d4) +
  514.       c1 * det3x3 (a2, a3, a4, b2, b3, b4, d2, d3, d4) -
  515.       d1 * det3x3 (a2, a3, a4, b2, b3, b4, c2, c3, c4);
  516.  
  517.     return ans;
  518. }
  519.  
  520.  
  521. double det3x3 (double a1, double a2, double a3, double b1, double b2,
  522.            double b3, double c1, double c2, double c3)
  523. {
  524.     double ans;
  525.  
  526.     ans = a1 * det2x2 (b2, b3, c2, c3)
  527.     - b1 * det2x2 (a2, a3, c2, c3)
  528.     + c1 * det2x2 (a2, a3, b2, b3);
  529.  
  530.     return ans;
  531. }
  532.  
  533.  
  534. double det2x2 (double a, double b, double c, double d)
  535. {
  536.     double ans;
  537.     ans = a * d - b * c;
  538.     return ans;
  539. }
  540.  
  541.